/*
 * miniFreeRTOS
 * Copyright (c) 2024- yanl1229@163.com.
 * Written by yanl1229
 *
 * This code is licensed under the GPL.
 */
#include <stdlib.h>
#include <string.h>

#include "task.h"
#include "mm.h"

/**
 * 当前正在运行的任务描述符
 */
struct task_struct * volatile current = NULL;
/**
 * 空闲任务描述符
 */
static struct task_struct *idle_task = NULL;

/**
 * @queue: 就绪任务管理数组,每一个优先级拥有自己的管理队列
 */
struct rt_prio_array {
	volatile unsigned long bitmap;
	struct list_head queue[CONFIG_MAX_PRIORITIES];
    volatile unsigned long task_count;
};

static struct rt_prio_array rt_queue;

static struct list_head * volatile delay_task_list;
static struct list_head * volatile overflow_task_list;
static struct list_head * volatile death_task_list;
static struct list_head * volatile pend_sched_list;

/**
 * @jiffies: 当前系统tick计数器
 */
volatile uint32_t jiffies = 0;

static volatile long scheduler_stat = SCHED_STOP;
static volatile uint32_t pend_ticks = 0;

static volatile uint32_t next_timeout_tick = 0;
static volatile unsigned long sched_suspend_count = 0;

uint32_t get_tasks()
{
    return rt_queue.task_count;
}

/**
 * 获取最高优先级的任务
 */
static inline uint32_t get_leading_zero_count(uint32_t bitmap)
{
    uint32_t count;
    /* CLZ指令是从高位(31bit)向低位(0bit)进行统计寄存器中为0的个数 */
    __asm volatile ( "clz %0, %1" : "=r" (count) : "r" (bitmap) : "memory" );
    return count;
}

uint32_t get_high_priority()
{
    uint32_t highest_priority;

    highest_priority = 31 - get_leading_zero_count(rt_queue.bitmap);

    return highest_priority;
}

/**
 * 找到优先级最高的任务
 * current 保存优先级最高的任务
 * @highest_priority: 保存当前系统最高优先级
 */
static void pick_next_task()
{
    unsigned long highest_priority;
    struct list_head *queue_head;

    highest_priority = get_high_priority();
    queue_head =&rt_queue.queue[highest_priority];

    /* 设置下一个待执行的任务 */
    if (!list_is_empty(&queue_head))
       current = queue_head->front->next->priv;

}

struct list_head* get_queue_by_priority(uint8_t priority)
{
    if (priority < 0 || priority > CONFIG_MAX_PRIORITIES)
        return NULL;

    return &rt_queue.queue[priority];
}

static void error_loop(void)
{
    irq_mask();

    while (1);
}

/*
 * 创建任务时，初始化任务的栈
 * 任务栈的布局如下:
 *
 * ---------------------------------
 * |         xPSR                  |
 * ---------------------------------
 * |         任务函数入口            |
 * ---------------------------------
 * |         LR寄存器               |
 * ---------------------------------
 * |         R12                   |
 * ---------------------------------
 * |         R3                    |
 * ---------------------------------
 * |         R2                    |
 * ---------------------------------
 * |         R1                    |
 * ---------------------------------
 * |         R0                    |
 * ---------------------------------
 * |         EXC_RETURN            |
 * ---------------------------------
 * |         R11                   |
 * ---------------------------------
 * |         R10                   |
 * ---------------------------------
 * |         R9 ~ R6               |
 * ---------------------------------
 * |         R5                    |
 * ---------------------------------
 * |         R4                    |
 * ---------------------------------
 */
static uint32_t * init_task_stack(uint32_t *stack_top,
                                     void(task_fn)(void*),
                                     void *args)
{
    stack_top--;

    /* 保存psr寄存器需要设置的值, 在任务切换的时候，进行更新 */
    *stack_top = INITIAL_XPSR_VAL;
    stack_top--;
    /* 设置为任务入口函数地址 */
    *stack_top = ((uint32_t)task_fn) & INITIAL_EXC_RETURN_MSK;
    stack_top--;
    /* 设置LR寄存器值 */
    *stack_top = (uint32_t) error_loop;
    /* R12, R3, R2 and R1. */
    stack_top -= 5;
    /* R0 */
    *stack_top = (uint32_t) args;

    stack_top--;
    *stack_top = INITIAL_EXC_RETURN_MSK;
    /* R11, R10, R9, R8, R7, R6, R5 and R4. */
    stack_top -= 8;

    return stack_top;
}

static void init_new_task(void(task_fn)(void*),
                                  char *name,
                                  uint32_t lengtn,
                                  void *args,
                                  unsigned long prio,
                                  struct task_struct *task)
{
    uint32_t * stack_top;
    unsigned long i;

    /*
     * 栈向下生长(高地址往低地址)
     */
    {
        /* 设置栈顶的位置(高地址) */
        stack_top = &(task->stack[lengtn - (uint32_t)1]);
        /* 栈地址对齐到STACK_ALIGNMENT_MASK */
        stack_top = (uint32_t *)(((uint32_t) stack_top) & (~((uint32_t) STACK_ALIGNMENT_MASK)));
    }

    if (name != NULL) {
        for (i =  0; i < CONFIG_MAX_TASK_NAME_LEN; i++) {
            /* 保存任务名称 */
            task->name[i] = name[i];
            if (name[i] == (char) 0x00)
                break;

        }
        task->name[CONFIG_MAX_TASK_NAME_LEN - 1] = '\0';
    }

    if (prio >= CONFIG_MAX_PRIORITIES)
        prio = CONFIG_MAX_PRIORITIES - 1;


    task->prio = prio;
    task->static_prio = prio;
    task->in_ready = 0;

    task->run_node.priv = task;
    task->event_node.priv = task;
    task->delay_node.priv = task;

    /* 指向 任务栈中的R11的位置 */
    task->stack_top = init_task_stack(stack_top, task_fn, args);
}

static void __enqueue_task(struct task_struct * task)
{
    rt_queue.bitmap |= 1 << task->prio;
    list_add_tail(&rt_queue.queue[task->prio], &task->run_node);
    rt_queue.task_count++;
    task->in_ready = 1;
}

void dequeue_task(struct task_struct * task)
{
    /* 与task同优先级的任务在就绪队列中全部为空 */
    if (list_remove(&task->run_node) == 0)
    {
        /* 取消任务就绪标记 */
        rt_queue.bitmap &= ~(1UL << task->prio);
    }
    rt_queue.task_count--;
    task->in_ready = 0;
}

static int queue_is_empty(struct list_head *list)
{
    return (list->num == 0);
}

static int queue_length(struct list_head *list)
{
    return list->num;
}

void enqueue_task(struct task_struct * task)
{
    disable_interrupt();
    {
        /* 新创建的任务，加入就绪队列 */
        __enqueue_task(task);

        /* 系统当前没有任务运行 */
        if (current == NULL) {
            current = task;
        }
        else
        {
            /* 任务调度器还没有运行 */
            if (scheduler_stat == SCHED_STOP) {
                /* 如果新建任务的优先级，大于当前任务的优先级，将新创建的任务设置为当前任务 */
                if (task->prio >= current->prio)
                    current = task;
            }
        }


    }
    enable_interrupt();

    /* 调度器已经开始运行, 检查新创建的任务，是否可以抢占，当前正在运行的任务 */
    if (scheduler_stat == SCHED_RUNNIING) {
        /**
         * 新创建的任务优先级高于当前正在运行任务的优先级，触发抢占
         * 对于cortex-m系列的arm来说，就是触发PendSVHandler异常
         */
        if (task->prio > current->prio)
            yield();
    }
}

void sched_init(void)
{
    int i;

    /* 初始化优先级队列 */
    for (i = 0; i < CONFIG_MAX_PRIORITIES; i++) {
        list_init(&rt_queue.queue[i]);
    }

    delay_task_list = malloc(sizeof(struct list_head));
    list_init(delay_task_list);
    overflow_task_list = malloc(sizeof(struct list_head));
    list_init(overflow_task_list);
    death_task_list = malloc(sizeof(struct list_head));
    list_init(death_task_list);
    pend_sched_list = malloc(sizeof(struct list_head));
    list_init(pend_sched_list);
}

/**
 * 创建任务
 * @task_fn: 任务函数入口
 * @name： 任务名称
 * @length: 任务堆栈大小
 * @args: 传递给任务的参数
 * @prio： 任务优先级, 数字越大，优先级越大
 */
struct task_struct * task_create(void(task_fn)(void*),
                        char *name,
                        uint16_t length,
                        void *args,
                        unsigned long prio)
{
    struct task_struct *task = NULL;
    uint32_t *sp;

    /* 申请任务堆栈空间 */
    sp = kmalloc((((size_t)length) * sizeof(uint32_t)));
    if (sp != NULL) {
        task = (struct task_struct *)kmalloc(sizeof(struct task_struct));
        if (task != NULL) {
            memset((void *)task, 0x00, sizeof(struct task_struct));
            /* 设置新任务的堆栈 */
            task->stack = sp;
        }
        else
        {
            kfree(sp);
        }
    }

    if (task != NULL) {
        init_new_task(task_fn, name, length, args, prio, task);
        /* 新创建的任务，加入任务就绪队列 */
        enqueue_task(task);
    }

    return task;
}

static void free_task(struct task_struct *task)
{
    kfree(task->stack);
    kfree(task);
}

/**
 * 删除加入death_task_list队列的任务描述符占用的内存
 */
static void delete_zommbie_task(void)
{
    struct task_struct * task;

    while (queue_length(death_task_list) > 0)
    {
        disable_interrupt();
        /* 从终止队列中删除 */
        task = (struct task_struct *)get_owner_of_first_entry(death_task_list);
        list_remove(&task->run_node);

        enable_interrupt();
        /* 释放任务占用的内存 */
        free_task(task);
    }

}

static void idle_task_fn(void *args)
{
    for (; ;)
    {
        /* 删除僵尸任务 */
        delete_zommbie_task();

        /*
         * 开启抢占的情况下，用户空间创建了与idle相同优先级的任务，则用户空间的任务是可以
         * 使用空闲任务的时间片, 有和空闲任务相同优先级的用户任务，则空闲任务让出cpu
         */
        if (queue_length(&rt_queue.queue[CONFIG_TASK_IDLE_PRIORITY]) > 1)
            yield();
    }
}

void sched_tick(void)
{
    /**
     * systick中断响应中，为了提高实时性，不禁止系统中断，并且临时提高
     * 中断优先级，以便可以快速响应其他中断
     */
    irq_mask();
    {
        /* 手动触发PendSV interrupt 进行任务切换 */
        if (update_tick() != 0)

            raw_writel(NVIC_PENDSVSET_BIT, NVIC_INT_CTRL_REG);
    }
    /* 恢复中断优先级 */
    irq_unmask();
}

static void setup_systick_timer(void)
{
    raw_writel(0, NVIC_SYSTICK_CTRL_REG);
    raw_writel(0, NVIC_SYSTICK_CURRENT_VALUE_REG);

    raw_writel(((CONFIG_SYSTICK_CLOCK/HZ )-1), NVIC_SYSTICK_LOAD_REG);
    raw_writel((NVIC_SYSTICK_CLK_BIT|NVIC_SYSTICK_INT_BIT|NVIC_SYSTICK_ENABLE_BIT), NVIC_SYSTICK_CTRL_REG);
}

static void start_first_task(void)
{
    __asm volatile (
        " ldr r0, =0xE000ED08 	\n"     /* 获取VTOR寄存器的地址 */
        " ldr r0, [r0] 			\n"     /* 获取保存在VTOR中的值(中断向量表的地址) */
        " ldr r0, [r0] 			\n"     /* 从中断向量表中获取msp寄存的值 */
        " msr msp, r0			\n"     /* 初始化msp寄存器 */
        " cpsie i				\n"     /* 使能系统中断 */
        " cpsie f				\n"
        " dsb					\n"
        " isb					\n"
        " svc 0					\n"     /* 触发SVC中断 */
        " nop					\n"
        );
}

long _sched_start(void)
{
    uint32_t reg = raw_readl(NVIC_SHPR3_REG);

    reg |= (NVIC_PENDSV_PRI|NVIC_SYSTICK_PRI);
    /* 设置PendSV和Systick中断的优先级(最低) */
    raw_writel(reg, NVIC_SHPR3_REG);


    setup_systick_timer();

    interrupt_nest_count = 0;

    start_first_task();

    /* Should not get here! */
    return 0;
}

void sched_start(void)
{

    /* 创建系统空闲任务. */
    idle_task = task_create(idle_task_fn,
                               "idle",
                               CONFIG_MINIMAL_STACK_SIZE,
                               NULL,
                               CONFIG_TASK_IDLE_PRIORITY);

    if (idle_task == NULL)
        return ;

    disable_interrupt();

    next_timeout_tick = MAX_DELAY;
    /* 调度器开始运行 */
    scheduler_stat = SCHED_RUNNIING;
    /* 初始化系统tick值 */
    jiffies = 0;

    _sched_start();
}

/**
 * 检查调度器状态
 */
long get_sched_stat(void)
{
    return scheduler_stat;
}


static void reset_next_timeout_tick(void)
{
    /* 延迟队列为空 */
    if (queue_is_empty(delay_task_list) != 0)
        next_timeout_tick = MAX_DELAY;
    else
        next_timeout_tick = get_value_of_first_entry(delay_task_list);
}

/**
 * 当jiffies溢出时候，交换pdelay_task_list和pverflow_task_list队列
 */
static void swap_delay_list()
{
    struct list_head * list;

    list = delay_task_list;
    delay_task_list = overflow_task_list;
    overflow_task_list = list;
    reset_next_timeout_tick();
}

long update_tick(void)
{
    struct task_struct * task;
    uint32_t value;
    long need_preempt = 0;

    /* 调度器如果没有被挂起 */
    if (sched_suspend_count != 1) {
        /*
         * 累加系统时钟数, 时钟中断中并没有禁止中断，因此可能会发生其他中断，打断
         * 时钟中断，导致jiffies出现不一致，因此先保存操作
         */
        uint32_t jiffies32 = jiffies + 1;

        jiffies = jiffies32;

        /* 系统时钟计数器发生溢出 */
        if (jiffies32 == 0)
            swap_delay_list();

        /* 遇见下一个任务超时的时间点 */
        if (jiffies32 >= next_timeout_tick) {
            for (; ;)
            {
                /* 延迟队列为空 */
                if (queue_is_empty(delay_task_list) != 0)
                {
                    /* 设置下一个任务超时的时间为无限大 */
                    next_timeout_tick = MAX_DELAY;
                    break;
                }
                else
                {
                    /* 延迟队列是有序的，链表头部的任务，超时时间值是最小的 */
                    task = (struct task_struct *)get_owner_of_first_entry(delay_task_list);
                    /* 获取任务的超时时间值 */
                    value = task->delay_node.value;

                    /* 没有超时 */
                    if (value > jiffies32)
                    {
                        /* 设置下一次的超时时间点. */
                        next_timeout_tick = value;
                        break;
                    }

                    /* 任务超时，从延迟队列中删除 */
                    list_remove(&(task->delay_node));

                    /* 任务加入到就绪队列 */
                    __enqueue_task(task);
                    /*
                     * 抢占式模型中，只要就绪任务的优先级大于当前任务
                     * 当前任务可以被抢占
                     */
                    if (task->prio > current->prio)
                        need_preempt = 1;
                }
            }
        }

        /* 抢占内核模型中, 相同优先级的任务，分时间片轮转 */
        if (queue_length(&rt_queue.queue[current->prio]) > 1)
            need_preempt = 1;

    } else {
        /* 调度器被挂起 */
        ++pend_ticks;
    }

    return need_preempt;
}

uint32_t get_tick(void)
{
    return jiffies;
}

void switch_context(void)
{
    /* 任务调度器没有被挂起, 选择优先级最高的任务 */
    if (sched_suspend_count == 0)
        pick_next_task();

}

/**
 * 挂起调度器,可以递归调用
 */
void sched_suspend(void)
{
    scheduler_stat = SCHED_SUSPEND;
    ++sched_suspend_count;
    memory_barrier();
}

/**
 * 恢复调度器的运行
 */
long sched_resume(void)
{
    long need_preempt = 0;
    struct task_struct *task;

    disable_interrupt();
    {
        --sched_suspend_count;

        /* 恢复调度器的执行 */
        if (sched_suspend_count == 0)
        {
            scheduler_stat = SCHED_RUNNIING;
            if (rt_queue.task_count > 0)
            {
                /* 等待调度任务队列不为空 */
                while (!queue_is_empty(pend_sched_list))
                {
                    /* 选择优先级最高的任务 */
                    task = (struct task_struct *)get_owner_of_first_entry(pend_sched_list);
                    /* 任务从等待队列中删除 */
                    list_remove(&task->event_node);
                    memory_barrier();
                    /* 加入到就绪队列 */
                    __enqueue_task(task);

                    /**
                     * task的优先级大于当前正在运行任务的优先级，当前运行任务可以
                     * 被抢占
                     */
                    if (task->prio >= current->prio)
                        need_preempt = 1;
                }

                /**
                 * 系统调度器被挂起后，产生时钟中断，累计
                 * 错过时钟更新的次数
                 */
                uint32_t pend_count = pend_ticks;

                /* 更新由于调度器被挂起后，错过的调度次数 */
                if (pend_count > 0)
                {
                    do
                    {
                        if (update_tick() == 1)
                            need_preempt = 1;

                        --pend_count;
                    } while (pend_count > 0U);

                    pend_ticks = 0;
                }


                /* 开启抢占的情况下，当前任务可以被抢占 */
                if (need_preempt == 1)
                    yield();
            }
        }
    }
    enable_interrupt();

    return need_preempt;
}

static void add_delayed_list(uint32_t relative_timeout)
{
    uint32_t absolute_timeout;
    uint32_t jiffies32 = jiffies;

    /* 当前任务从就绪队列删除 */
    dequeue_task(current);

    /* 计算任务绝对唤醒的时间点 */
    absolute_timeout = jiffies32 + relative_timeout;

    /* 设置任务延迟超时时间 */
    current->delay_node.value = absolute_timeout;

    /* 发生溢出 */
    if (absolute_timeout < jiffies32)
    {
        list_add_order(overflow_task_list, &current->delay_node);
    }
    else
    {
        list_add_order(delay_task_list, &current->delay_node);

        if (absolute_timeout < next_timeout_tick)
            /* 更新下一个取消任务阻塞的时间点 */
            next_timeout_tick = absolute_timeout;
    }
}

/**
 * 相对延迟
 * @ticks: 系统相对节拍数
 */
void task_delay(uint32_t ticks)
{
    long need_preempt = 1;

    if (ticks > 0)
    {
        /* 临时挂起任务调度 */
        sched_suspend();
        add_delayed_list(ticks);
        /* 恢复任务调度 */
        need_preempt = sched_resume();
    }

    /* 强制触发任务调度，需要延迟的任务被立刻挂起 */
    if (need_preempt == 0)
        yield();
}

/**
 * 删除任务
 * @tsk: 待删除的任务
 */
void task_delete(struct task_struct *tsk)
{
    if (tsk == NULL)
        return ;

    disable_interrupt();

    /*  任务从就绪队列删除 */
    dequeue_task(tsk);
    /* 待删除任务是当前正在运行的任务，延迟删除 */
    if (tsk == current)
        list_add_tail(death_task_list, &tsk->run_node);
    else
        reset_next_timeout_tick();

    enable_interrupt();

    /* 不是正在运行的任务，立刻删除 */
    if (tsk != current)
        free_task(tsk);

    if (scheduler_stat == SCHED_RUNNIING) {
        /* 删除当前正在运行的任务时, 触发一次任务调度 */
        if (tsk == current)
            yield();

    }
}

/**
 * 重新计算超时时间，并返回是否超时
 */
uint32_t calculate_timeout(uint32_t *timeout, uint32_t *wait_ticks)
{
    uint32_t ret;

    disable_interrupt();
    {
        uint32_t jiffies32 = jiffies;
        /* 计算出timeout初始设置到现在，已经经过的tick */
        uint32_t elapsed_time = jiffies32 - *timeout;

        if (*wait_ticks == MAX_DELAY)
        {
            ret = 0;
        }
        else if (elapsed_time < *wait_ticks)
        {
            *wait_ticks -= elapsed_time;
            *timeout = jiffies;
            ret = 0;
        }
        else
        {
            *wait_ticks = (uint32_t)0;
            ret = 1;
        }
    }
    enable_interrupt();

    return ret;
}

uint32_t remove_from_event_queue(struct list_head *list)
{
    struct task_struct * task;
    uint32_t need_preempt;

    task = (struct task_struct *)get_owner_of_first_entry(list);
    list_remove(&task->event_node);

    /* 如果调度器正在运行中 */
    if (scheduler_stat == SCHED_RUNNIING)
    {
        /* 从延迟队列删除 */
        list_remove(&task->delay_node);
        /* 任务重新加入就绪队列 */
        __enqueue_task(task);
    }
    else
    {
        /* 调度器被挂起状态，将任务加入到等待加入就绪队列链表 */
        list_add_tail(pend_sched_list, &task->event_node);
    }

    /* task任务优先级比当前正在运行任务的优先级要高，当前任务被抢占 */
    if (task->prio > current->prio)
        need_preempt = 1;
    else
        need_preempt = 0;

    return need_preempt;
}

void add_to_event_queue(struct list_head *list, uint32_t ticks)
{
    list_add_tail(list, &current->event_node);
    add_delayed_list(ticks);
}
